
#include "dmpKey.h"
#include "dmpmap.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "delay.h"
#include "soft_i2c.h"
#include "string.h"

#define DEFAULT_MPU_HZ          (20)
#define q30  1073741824.0f

struct platform_data_s {
    signed char orientation[9];
};

static struct platform_data_s gyro_pdata = {
     .orientation = 
               { 1, 0, 0,
                 0, 1, 0,
                 0, 0, 1}
};

static mpu6050_t  mpu6050 = {0};
static struct int_param_s int_param;

unsigned short acc_sens = 1;
float gyro_sens = 1.0;

void mpu6050_reg_tab_cb(void (*func)(unsigned char, unsigned char))
{
	dmp_register_tap_cb(func);
}

void mpu6050_init(void)
{ 
	soft_i2c_init(SOFT_I2C_2);

	if(!mpu_init(&int_param))     //mpu初始化
	{
		if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))    log_statue( 1 );		//设置需要的传感
		if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) log_statue( 2 );		//设置fifo
		if(mpu_set_sample_rate(DEFAULT_MPU_HZ))              log_statue( 3 );		//设置采集样率

		if(dmp_load_motion_driver_firmware())                log_statue( 4 );		//加载dmp固件
		
		if(dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_PEDOMETER));
		if(dmp_set_fifo_rate(DEFAULT_MPU_HZ))    			 log_statue( 5 );		//设置速率		
		if(mpu_set_dmp_state(1))        					 log_statue( 6 );       //使能

        mpu_get_accel_sens(&acc_sens);
        mpu_get_gyro_sens(&gyro_sens);

	    dmp_set_pedometer_step_count(0);
        dmp_set_pedometer_walk_time(0);																		  
	}
    
}

void mpu_set_pedometer_step_count(unsigned long count)
{
    dmp_set_pedometer_step_count(count);
    dmp_set_pedometer_walk_time(0);	
}

unsigned char mpu6050_update(void)
{   
	short sensors;
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
	unsigned long sensor_timestamp;
	unsigned char more;
	long quat[4];
	if(dmp_read_fifo(mpu6050.gryo, mpu6050.acc, quat, &sensor_timestamp, &sensors, &more)) 
		return 1;      
	if (sensors & INV_WXYZ_QUAT)
	{    
		q0 = quat[0] / q30;
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30; 

		mpu6050.ouler[0] = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		mpu6050.ouler[1] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		mpu6050.ouler[2] = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw

	}   
	
	static uint32_t old_time = 0;
	if(time_to_now(old_time) >= 1000)
	{
		uint32_t temp;
		dmp_get_pedometer_step_count(&mpu6050.pedometer);
		dmp_get_pedometer_walk_time(&temp);
		get_ms(old_time);
	}

	return 0;
}

void mpu6050_read(mpu6050_t  *mpu_dat)
{
	memcpy( mpu_dat , &mpu6050 , sizeof(mpu6050_t));
}

int Sensors_I2C_ReadRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned short len, unsigned char *data_ptr)
{
	int ret = 0;
	soft_i2c_start(SOFT_I2C_2, slave_addr);
	if(	soft_i2c_read(SOFT_I2C_2, reg_addr, data_ptr, len) != len)
	{
		ret = 1;
	}
	soft_i2c_stop(SOFT_I2C_2);
	return ret;
}

int Sensors_I2C_WriteRegister(unsigned char slave_addr, unsigned char reg_addr,unsigned short len,const unsigned char *data_ptr)
{
	int ret = 0;
	soft_i2c_start(SOFT_I2C_2, slave_addr);
	if(	soft_i2c_write(SOFT_I2C_2, reg_addr, data_ptr, len) != len)
	{
		ret = 1;
	}
	soft_i2c_stop(SOFT_I2C_2);
	return ret;
}


void log_debug(char *format, ...)
{
	
}

void log_statue(unsigned int statue)
{

	ui_debug("**** %d ****", statue);
	while(1);
}


